#requires the robot team module
require 'control_lib/level2/main/robot_team.rb'

#requires the robot teammap module
require 'control_lib/level2/support/teammap.rb'



#require the math library
def de2rad(float)
  rad=float
  pi=3.14159265
  if (rad>0)
    return ((2*pi)/360)*rad
  else
    return 2*pi+(((2*pi)/360)*rad)
  end
end

################ PROGRAM STARTS HERE ########################

team1= RobotTeam.new("searchteam","10.1.157.59",3000)
#team1.addrobot("USARBot.P2AT","-6.0,-2.0,1.8","robot1")
team1.addrobot("USARBot.P2AT","0.748,-7.282,-0.446","robot1")

sleep(1)

map=Teammap.new(team1)
map.autoupdatemap(true)
map.autologmap(true)


team1.robotno.each{|robot|
  robot.colguardmode
}


cbot=team1.robotno[0]

#Main Loop
oldtime=Time.now
totaltime=0.0
checkno=20
while(totaltime<=60.0*12.0)

#start by backing up a bit
cbot.smartback(0.25,2.0)

#find max 
scans=cbot.rangescan.size
maxscans=Array.new()
maxdeg=Array.new()
scanarray=Array.new(cbot.rangescan)
  
checkno.times{
max=0.0
index=-1
scanarray.each_with_index{|rangestr,deg|
  range=rangestr.to_f
  if(range>max and range<19.9)
    max=range
    index=deg
  end
 }
scanarray.delete_at(index) #remove from list
maxscans<<max
maxdeg<<(index)
}

#create roulette wheel
roullet=Array.new(maxscans)

#find sum
sum=0.0
roullet.each{|i|
  sum=sum+i
}

#change to percents 
roullet.collect!{|member|
  member=(member/sum)*100
}

#change to sumation of percents
sum=0.0
roullet.collect!{|member|
  member=member+sum
  sum=member
}

#randomize and choose
choice=rand(100)
choicedeg=0.0
roullet.each_with_index {|member,index|
    if(choice<member)
      choicedeg=maxdeg[index]
      choice=maxscans[index]
      break
    end
}
#the chosen deg is choicedeg and distance is choice
puts choice,choicedeg 
#back up a bit

if(choicedeg<=90)
  cbot.smartright(de2rad(90-choicedeg))
else
  cbot.smartleft(de2rad(choicedeg-90))
end

puts choice
dist=(choice-0.5)
if(choice<1.0)
  dist=choice
end
cbot.smartfor(dist,1.0)
#cbot.smartfor(2.0,1.0)
puts "calculatings"
sleep(1.0)

end  #while
 

 
 


sleep(20)